#include "ros/ros.h"
#include <string>
#include <sstream>
// dji sdk
#include <dji_sdk/Activation.h>
#include <dji_sdk/DroneTaskControl.h>
#include <dji_sdk/SDKControlAuthority.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/NavSatFix.h>
#include <dji_sdk/SetLocalPosRef.h>
#include <geometry_msgs/PointStamped.h>
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/QuaternionStamped.h"
#include <tf/tf.h>

#include "radio_proxy/Command.h"
#include "radio_proxy/FlightData.h"

// constant
enum Mission {
    IDLE,
    TAKINGOFF,
    TAKEOFFED,
    MOVING,
	FOLLOW,
    LANDING,
	WAITGOAL
};

// variable
int id;
int leaderID;

int mission = IDLE;


geometry_msgs::PointStamped local_position;
double target_x;
double target_y;
double target_z;
double target_yaw;

#define C_EARTH (double)6378137.0
#define C_PI (double)3.141592653589793
#define DEG2RAD(DEG) ((DEG) * ((C_PI) / (180.0)))
const float deg2rad = C_PI/180.0;
const float rad2deg = 180.0/C_PI;
bool enableFollow = true; // please add to param, noted by zsp
double formation[4][4] = {{0.0,0.0,0.0,0.0}, {0.0,0.0,0.0,0.0},{-8.0,-8.0,0.0,0.0},{8.0,-8.0,0.0,0.0}}; //XYZYaw, vinson modified
radio_proxy::FlightData leaderStatus;
sensor_msgs::NavSatFix currentGps;
double timeLeaderStatusInfoCur = -1.0;

// service
ros::ServiceClient     drone_activation_service;
ros::ServiceClient     sdk_ctrl_authority_service;
ros::ServiceClient     drone_task_service;
ros::ServiceClient     set_local_pos_reference;

// subcriber
ros::Subscriber localPosition;
ros::Subscriber cmdSub;
ros::Subscriber leaderGpsSub;
ros::Subscriber gpsSub;

// publisher
ros::Publisher ctrlPosYawPub;

// timer
ros::Timer targetPubTimer;


// bool 返回值由于标志是否处理成功
void cmdCB(const radio_proxy::Command::ConstPtr& msg){

    dji_sdk::Activation activation;
    dji_sdk::SDKControlAuthority sdkAuthority;
    dji_sdk::DroneTaskControl droneTaskControl;
    dji_sdk::SetLocalPosRef localPosReferenceSetter;

    if (msg->target_id != id && msg->target_id != 255){
        return;
    }

    uint8_t cmd = msg->command;

    switch(cmd){

        case radio_proxy::Command::TAKEOFF :

			if(mission != IDLE)
			{
				break;
			}

            mission = TAKINGOFF;

            // set local start point

            set_local_pos_reference.call(localPosReferenceSetter);

            // activate
            
            drone_activation_service.call(activation);
            if (!activation.response.result)
            {
                ROS_ERROR("activating failed.");
            }
            
            // get authority
            
            sdkAuthority.request.control_enable = 1;
            sdk_ctrl_authority_service.call(sdkAuthority);
            if (!sdkAuthority.response.result)
            {
                ROS_ERROR("authorizing failed.");
                
            }

            // take off
            
            droneTaskControl.request.task = 4;
            drone_task_service.call(droneTaskControl);
            if (!droneTaskControl.response.result)
            {
                ROS_ERROR("take off failed.");
            }

            // mission = TAKEOFFED; //zsp modify
			mission = WAITGOAL;
            break;

        case radio_proxy::Command::SETPOINT_LOCAL :

            // mission = MOVING; //zsp modify
			if(mission != WAITGOAL)
			{
				break;
			}

            target_x = msg->x;
            target_y = msg->y;
            target_z = msg->z;
            target_yaw = msg->yaw;

		if (id == 1){
			mission = MOVING;
		}
		else{
			mission = FOLLOW;
		}
			
            break;

        case radio_proxy::Command::LAND :

            mission = LANDING;

            droneTaskControl.request.task = 6;
            drone_task_service.call(droneTaskControl);
            if (!droneTaskControl.response.result)
            {
                ROS_ERROR("land failed.");
            }

            mission = IDLE;

            break;
            
    }

}


void local_position_callback(const geometry_msgs::PointStamped::ConstPtr& msg) 
{
	local_position = *msg;
}


void local_offset_from_gps_offset(double& x, double& y, double& z, sensor_msgs::NavSatFix& target, radio_proxy::FlightData& origin)
{
    double deltaLon = target.longitude - origin.longitude;
    double deltaLat = target.latitude - origin.latitude;

    y = deltaLat * deg2rad * C_EARTH;
    x = deltaLon * deg2rad * C_EARTH * cos(deg2rad*target.latitude);
    z = target.altitude - origin.altitude;

}


void leader_GPS_cb(const radio_proxy::FlightData::ConstPtr& gpsMsg) 
{
	if(gpsMsg->id != leaderID)
	{
		ROS_INFO("\033[1;31m rcv flight data msg not from leader!\033[0m");
		return;
	}

	//if(abs(gpsMsg->header.stamp.toSec() - timeLeaderStatusInfoCur) < 1e-3 || !enableFollow || mission != WAITGOAL)
	//{
	//	ROS_INFO("\033[1;31m rcv flight data msg, wait for [flag]enableFollow or [mode]waitgoal!\033[0m");
	//	return;
	//}

	timeLeaderStatusInfoCur = gpsMsg->header.stamp.toSec();
	leaderStatus = *gpsMsg;
	//mission = FOLLOW;
}


void gps_position_cb(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
    currentGps = *msg;
}


void targetPubTimerCallback(const ros::TimerEvent& event)
{
	sensor_msgs::Joy controlPosYaw;
	static double cmdX, cmdY, cmdZ;

	// need to test
	if(mission == FOLLOW)
	{
		// remaining offset
		local_offset_from_gps_offset(cmdX, cmdY, cmdZ, currentGps, leaderStatus);
		cmdX = formation[id][0]- cmdX;
		cmdY = formation[id][1]- cmdY;
		cmdZ = local_position.point.z;
		target_yaw = 0.0;

		// if(debug_mode) info_counter++;
		// if(info_counter > 50)
		// {
		// 	double yawInRad = to_euler_angle(current_atti).z;
		// 	info_counter = 0;
		// 	ROS_INFO("dx=%f, dy=%f, dz=%f, dyaw=%f ...", cmdX,cmdY,cmdZ,target_yaw);
		// }

		controlPosYaw.axes.push_back(cmdX);
        controlPosYaw.axes.push_back(cmdY);
        controlPosYaw.axes.push_back(cmdZ);
        controlPosYaw.axes.push_back(target_yaw);

        ctrlPosYawPub.publish(controlPosYaw);

		//if(abs(cmdX) < 5e-2 && abs(cmdY) < 5e-2 && abs(local_position.point.z - cmdZ) < 5e-2)
		//{
		//	mission == WAITGOAL;
		//}
	}
    else if(mission == MOVING)
	{
		cmdX = target_x - local_position.point.x;
		cmdY = target_y - local_position.point.y;
		cmdZ = target_z;

        controlPosYaw.axes.push_back(cmdX);
        controlPosYaw.axes.push_back(cmdY);
        controlPosYaw.axes.push_back(cmdZ);
        controlPosYaw.axes.push_back(target_yaw);

        ctrlPosYawPub.publish(controlPosYaw);

		// zsp modify
		if(abs(cmdX) < 5e-2 && abs(cmdY) < 5e-2 && abs(local_position.point.z - cmdZ) < 5e-2)
		{
			mission == WAITGOAL;
		}
    }
}


int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    ros::init(argc,argv,"dji_test_node_zsp");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");

    private_nh.param<int>("id", id, 1);
    private_nh.param<int>("leaderID", leaderID, 1);
    private_nh.param<bool>("enableFollow", enableFollow, true); // vinson add

    //service
    drone_activation_service = nh.serviceClient<dji_sdk::Activation>("dji_sdk/activation");
    sdk_ctrl_authority_service = nh.serviceClient<dji_sdk::SDKControlAuthority>("dji_sdk/sdk_control_authority");
    drone_task_service =nh.serviceClient<dji_sdk::DroneTaskControl>("dji_sdk/drone_task_control");
    set_local_pos_reference    = nh.serviceClient<dji_sdk::SetLocalPosRef> ("dji_sdk/set_local_pos_ref");
    // subsciber
    localPosition = nh.subscribe("dji_sdk/local_position", 10, &local_position_callback);
    cmdSub = nh.subscribe("/dji_proxy/command", 10, &cmdCB);
	leaderGpsSub = nh.subscribe<radio_proxy::FlightData>("/dji_proxy/leader/flight_data", 1, &leader_GPS_cb);
	gpsSub = nh.subscribe<sensor_msgs::NavSatFix>("/dji_sdk/gps_position", 10, &gps_position_cb);

    // publisher
    ctrlPosYawPub = nh.advertise<sensor_msgs::Joy>("dji_sdk/flight_control_setpoint_ENUposition_yaw", 10);
    // timer
    targetPubTimer = nh.createTimer(ros::Duration(0.1), targetPubTimerCallback);


    ros::spin();
    return 0;
}
